#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import time
import signal
import sys
import re
from datetime import datetime
from threading import Lock

import paho.mqtt.client as mqtt

import rclpy
from rclpy.node import Node
from rclpy.qos import qos_profile_sensor_data

# 按你的要求：消息包改为 my_custom_msgs.msg
from my_custom_msgs.msg import TrajectoryInfo, TimeInfo


# ---------- 时间解析工具 ----------
def parse_current_time(cur_time: str) -> datetime:
    """
    支持：
      - '20250824223754'
      - '2025年08月24日22:37:54'
      - 以及能提取出 6 个数字（年/月/日/时/分/秒）的字符串
    """
    if not isinstance(cur_time, str) or not cur_time.strip():
        raise ValueError("empty current_time")

    s = cur_time.strip()
    # 1) 先尝试纯数字
    try:
        return datetime.strptime(s, "%Y%m%d%H%M%S")
    except ValueError:
        pass
    # 2) 再尝试“中文年月日 + : 分隔时分秒”
    try:
        return datetime.strptime(s, "%Y年%m月%d日%H:%M:%S")
    except ValueError:
        pass
    # 3) 回退：正则提取 6 个数字段
    nums = re.findall(r"\d+", s)
    if len(nums) >= 6:
        y, M, d, H, m, S = map(int, nums[:6])
        return datetime(y, M, d, H, m, S)
    raise ValueError(f"Unsupported current_time format: {cur_time!r}")


def parse_elapsed_time(elap: str) -> datetime:
    """
    解析 elapsed_time 为“时间量”的容器（用 datetime 只是为了取 .hour/.minute/.second）：
      - 'HH:MM:SS'
      - 'HH时MM分SS秒'
      - 或者能提取出 3 个数字（时/分/秒）的字符串
    """
    if not isinstance(elap, str) or not elap.strip():
        raise ValueError("empty elapsed_time")

    s = elap.strip()
    # 1) 标准 HH:MM:SS
    try:
        return datetime.strptime(s, "%H:%M:%S")
    except ValueError:
        pass
    # 2) 中文“时分秒”
    m = re.match(r"^\s*(\d+)\s*时\s*(\d+)\s*分\s*(\d+)\s*秒\s*$", s)
    if m:
        H, M, S = map(int, m.groups())
        return datetime(1900, 1, 1, H, M, S)
    # 3) 回退：提取 3 段数字
    nums = re.findall(r"\d+", s)
    if len(nums) >= 3:
        H, M, S = map(int, nums[:3])
        return datetime(1900, 1, 1, H, M, S)
    raise ValueError(f"Unsupported elapsed_time format: {elap!r}")


class MqttPublisherNode(Node):
    def __init__(self):
        super().__init__('mqtt_publisher')

        # ====== 订阅缓存 ======
        self.current_trajectory = 0
        self.total_executions = 0
        self.current_time_str = ""
        self.elapsed_time_str = ""
        self._lock = Lock()

        # ====== ROS 2 订阅 ======
        self.create_subscription(
            TrajectoryInfo, 'trajectory_info',
            self._trajectory_info_cb, qos_profile_sensor_data
        )
        self.create_subscription(
            TimeInfo, 'time_info',
            self._time_info_cb, qos_profile_sensor_data
        )

        # ====== MQTT 配置（xyyl配置）======
        self.broker = "cscserver.cn"
        self.port = 1883
        self.topic = "/iot/KT4201/realtimeData"
        self.username = "vipxyyl"
        self.password = "vipxyyl"
        self.client_id = "KT4201"

        # ====== 创建 MQTT 客户端并连接 ======
        self.client = mqtt.Client(self.client_id)
        self.client.username_pw_set(self.username, self.password)
        self.client.on_connect = self._on_connect
        # self.client.on_message = self._on_message
        self.client.on_log = self._on_log
        self._mqtt_connected = False

        try:
            self.client.connect(self.broker, self.port, keepalive=60)
            self.client.loop_start()
        except Exception as e:
            self.get_logger().error(f"MQTT 连接异常: {e}")

        # ====== 定时器：每 10s 发送一次（与你 rospy 版一致）======
        self.create_timer(10.0, self._on_timer)

        self.get_logger().info("ROS2 mqtt_publisher 已启动。")

    # ---------- MQTT 回调 ----------
    def _on_connect(self, client, userdata, flags, rc):
        ok = (rc == 0)
        self._mqtt_connected = ok
        msg = f"MQTT 连接{'成功' if ok else '失败'}"
        print(msg)
        (self.get_logger().info if ok else self.get_logger().error)(msg)
        if ok:
            client.subscribe(self.topic)

    def _on_message(self, client, userdata, msg):
        print(f"收到下行: topic={msg.topic} payload={msg.payload}")

    def _on_log(self, client, userdata, level, buf):
        if level == mqtt.MQTT_LOG_ERR:
            print(f"[MQTT 错误] {buf}")
        elif level == mqtt.MQTT_LOG_WARNING:
            print(f"[MQTT 警告] {buf}")
        # elif level == mqtt.MQTT_LOG_INFO:
        #     print(f"[MQTT 信息] {buf}")

    # ---------- ROS 2 订阅回调 ----------
    def _trajectory_info_cb(self, msg: TrajectoryInfo):
        with self._lock:
            self.current_trajectory = int(msg.current_trajectory)
            self.total_executions = int(msg.total_executions)

    def _time_info_cb(self, msg: TimeInfo):
        with self._lock:
            self.current_time_str = str(msg.current_time)
            self.elapsed_time_str = str(msg.elapsed_time)

    # ---------- 定时打包发送 ----------
    def _on_timer(self):
        # 取快照 & 清空（模仿你原来 while 每圈清空一次）
        with self._lock:
            cur_traj = self.current_trajectory
            tot_exec = self.total_executions
            cur_time = self.current_time_str
            elap_time = self.elapsed_time_str
            self.current_time_str = ""
            self.elapsed_time_str = ""

        try:
            # 若两个时间都有内容，就解析并打包真实数据，否则发默认
            if cur_time and elap_time:
                current_time_dt = parse_current_time(cur_time)
                elapsed_time_dt = parse_elapsed_time(elap_time)

                year = current_time_dt.year % 100
                month = current_time_dt.month
                day = current_time_dt.day
                hour = current_time_dt.hour
                minute = current_time_dt.minute

                hours = elapsed_time_dt.hour
                minutes = elapsed_time_dt.minute
                seconds = elapsed_time_dt.second

                data = bytearray([
                    cur_traj & 0xFF, tot_exec & 0xFF,
                    year, month, day, hour, minute,
                    hours & 0xFF, minutes & 0xFF, seconds & 0xFF,
                    0xcd, 0xdc
                ])
                header_hex = "0g02"  # 保持你的“独立 header”（注意：并非标准 hex）
                data_hex = data.hex()
                full_message = header_hex + data_hex
            else:
                print("设备离线……发送默认数据")
                data = bytearray([
                    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
                    0x00, 0x00, 0x00, 0x00, 0x00, 0xcd, 0xdc
                ])
                header_hex = "0g02"
                data_hex = data.hex()
                full_message = header_hex + data_hex

        except Exception as e:
            self.get_logger().error(f"打包数据异常: {e}")
            return

        # 发布 & 打印状态
        try:
            if not self._mqtt_connected:
                print("MQTT 未连接（仍尝试 publish）")
            self.client.publish(self.topic, payload=full_message, qos=0, retain=False)
            # print(f"[发送] connected={self._mqtt_connected} data={full_message}")
            self.get_logger().info(f"已发送数据: {full_message}")
        except Exception as e:
            self.get_logger().error(f"MQTT 发布异常: {e}")

    # ---------- 清理 ----------
    def close(self):
        try:
            self.client.loop_stop()
        except Exception:
            pass
        try:
            self.client.disconnect()
        except Exception:
            pass


def main():
    rclpy.init()
    node = MqttPublisherNode()

    def _sigint_handler(sig, frame):
        print("正在停止...")
        node.close()
        rclpy.shutdown()
        sys.exit(0)

    signal.signal(signal.SIGINT, _sigint_handler)

    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.close()
        node.destroy_node()
        rclpy.shutdown()


if __name__ == "__main__":
    main()
